package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.templates.commands.ArmExtend;
import edu.wpi.first.wpilibj.templates.commands.ArmStow;
import edu.wpi.first.wpilibj.templates.commands.ConveyorLiftandLatch;
import edu.wpi.first.wpilibj.templates.commands.FlywheelManual;
import edu.wpi.first.wpilibj.templates.commands.FlywheelPIDsetspeed;
import edu.wpi.first.wpilibj.templates.commands.FlywheelPIDuserset;
//import edu.wpi.first.wpilibj.networktables.NetworkTable;

public class OI {
    //private NetworkTable usersTable;
    //private double userTarget;

    // The left joystick for driving
    // The right joystick will be used for ball handling
    private Joystick leftstick = new Joystick(RobotMap.leftstick_port);
    private Joystick rightstick = new Joystick(RobotMap.rightstick_port);
    //private Joystick midstick = new Joystick(RobotMap.midstick_port);

    //left (driving) joystick buttons
    Button lftbtn7 = new JoystickButton(leftstick, 7),
           lftbtn8 = new JoystickButton(leftstick, 8),
           lftbtn9 = new JoystickButton(leftstick, 9),
           lftbtn10 = new JoystickButton(leftstick, 10),

    //right (ball) joystick buttons
           rtbtn1 = new JoystickButton(rightstick, 1),
           rtbtn6 = new JoystickButton(rightstick, 6),
           rtbtn7 = new JoystickButton(rightstick, 7),
           rtbtn8 = new JoystickButton(rightstick, 8),
            //buttons for testing flywheel pid... TAKE THESE OUT BEFORE COMPETITION
           rtbtn2 = new JoystickButton(rightstick, 2),
           rtbtn3 = new JoystickButton(rightstick, 3),
           rtbtn4 = new JoystickButton(rightstick, 4),
           rtbtn5 = new JoystickButton(rightstick, 5);
    
    //button for second drive joystick
           //mdbtn2 = new JoystickButton(midstick, 2);

    public OI(){
        //leftstick buttons 1 - 12...  1 is the trigger:
        //lftbtn7.whenPressed(new DriveToBalance());
        //lftbtn8.whenPressed(new DriveWithJoy());
        //lftbtn9.whileHeld(new ArmExtend());
        //lftbtn10.whileHeld(new ArmStow());

        //rightstick buttons 1 - 11...  1 is the trigger:
        rtbtn1.whileHeld(new ConveyorLiftandLatch());
        rtbtn6.whileHeld(new ArmExtend());
        rtbtn7.whileHeld(new ArmStow());
        rtbtn8.whenPressed(new FlywheelManual());
        //buttons for testing flywheel pid at set speeds (rpms):
        //COMMENTED OUT 8-MAR MU
        rtbtn2.whenPressed(new FlywheelPIDsetspeed(0));
        //rtbtn3.whenPressed(new FlywheelPIDsetspeed(500));
        //rtbtn4.whenPressed(new FlywheelPIDsetspeed(1000));
        //END TEST BUTTONS 8-MAR MU
        rtbtn5.whenPressed(new FlywheelPIDuserset());
    }


    //gets the value of the x axis on the left stick
    public double getleftstickX() {
        return leftstick.getX();
    }

    //gets the value of the y axis on the left stick
    public double getleftstickY() {
        return leftstick.getY();
    }

    //gets the value of the z (twist) axis on the left stick
    public double getleftstickTwist() {
        return leftstick.getTwist();
    }

    //gets the value of the throttle wheel on the left stick
    public double getleftstickThrottle(){
        return leftstick.getThrottle();
    }

    //gets the value of the x axis on the right stick
    public double getrightstickX() {
        return rightstick.getX();
    }

    //gets the value of the y axis on the right stick
    public double getrightstickY() {
        return rightstick.getY();
    }

    //gets the value of the throttle wheel on the right stick
    public double getrightstickThrottle(){
        return rightstick.getRawAxis(3);
    }
    //gets the value of the x axis on the middle stick
    /* public double getmidstickX() {
        return midstick.getX();
    }

    //gets the value of the y axis on the middle stick
    public double getmidstickY() {
        return midstick.getY();
    }*/
}

